/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera.*;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.image.NIVision.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    AxisCamera Camera;                                                            
    CriteriaCollection Collection;
    ColorImage Image;                                                            
    BinaryImage BI;
    
    public void robotInit() {
        Camera = AxisCamera.getInstance("10.26.58.11");                                        
        Camera.writeResolution(AxisCamera.ResolutionT.k640x480);                  
        Camera.writeBrightness(25);                                               
        Camera.writeCompression(0);                                               
        Collection = new CriteriaCollection();
        Collection.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 0, 50, false);

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
    }
    public void ImageProcessing(){
    try {
            Image = Camera.getImage();
            BI = Image.thresholdRGB(109, 225, 50, 121, 52, 124); 
            BI = BI.removeSmallObjects(false, 2);
            BI = BI.particleFilter(Collection);
            BI = BI.convexHull(false);
//          BI = BI.particleFilter(Collection);
            ParticleAnalysisReport[] reports = BI.getOrderedParticleAnalysisReports();
            ParticleAnalysisReport r1 = reports[0];
            ParticleAnalysisReport r2 = reports[1];
            ParticleAnalysisReport r3 = reports[2];
            ParticleAnalysisReport r4 = reports[3];
            double centerX = ((r1.center_mass_x_normalized+r2.center_mass_x_normalized+r3.center_mass_x_normalized+r4.center_mass_x_normalized)/4);
            

            DriverStationLCD.getInstance().updateLCD();  
            BI.particleFilter(Collection).free();
            BI.convexHull(false).free();
            BI.removeSmallObjects(false, 2).free();
            BI.free();
            Image.free();
            //Timer.delay(.25);
            
        } catch (AxisCameraException ex) {  
            ex.printStackTrace();
        } catch (NIVisionException ex) {
            ex.printStackTrace();
        }
    }
    
}
